#include <cstdio>
#include <cstdlib>
#include <unistd.h> // For usleep()
#include <sstream> // For ostringstream
#include <iomanip>

#include "WupperParser.h"
#include "WupperReceiver.h"
//#include "crc.h"

// ----------------------------------------------------------------------------

WupperParser::WupperParser()
{
  _blockSize      = 1024;
  _blockSizeCode  = (_blockSize/1024-1) << 8;
  _block          = 0;
  _blockElinknr   = -1;
  _chunkIndex     = -1;
  _WupperReceiver    = 0;
  _blocksToHandle = 0;
  _blocksHandled  = 0;
  _fpOut          = 0;
  _scaCrcErrors   = 0;
  _scaCmdErrors   = 0;

}

// ----------------------------------------------------------------------------

void WupperParser::setReceiver( WupperReceiver *r )
{
  _WupperReceiver = r;

  if( _WupperReceiver )
    {
      _blockSize = _WupperReceiver->blockSize();
      _blockSizeCode = (_blockSize/1024-1) << 8;
    }

  startRecvTime();
}

// ----------------------------------------------------------------------------

int WupperParser::displayBlock( uint8_t *block, int blocknr,
                             int      nr_of_bytes,
                             int      start_offset,
                             int      elink_filter,
                             int      word_size )
{


  printf( "==> BLOCK " );
  if( blocknr >= 0 ) printf( "%d ", blocknr );
  

  // Round start offset to nearest lower 16-byte multiple, larger than 0
  if( start_offset < 0 )
    start_offset = 0;
  start_offset = (start_offset/16) * 16;
  if( start_offset > _blockSize )
    start_offset = 0;

  // Adjust number of bytes to display if necessary
  int nr_of_bytes_from_start = 0;
  if( nr_of_bytes < 1 || nr_of_bytes > _blockSize )
    nr_of_bytes = _blockSize;
  // More bytes requested than remaining in this block?
  // -> it means we want to see also the beginning of the block
  if( start_offset + nr_of_bytes > _blockSize )
    {
      // Display part of the beginning of the block
      // (to see end of previous block combined with start of this block)
      nr_of_bytes_from_start = start_offset + nr_of_bytes - _blockSize;
      // Round off to a multiple of 16
      nr_of_bytes_from_start = ((nr_of_bytes_from_start+15)/16)*16;

      // Display from the start of the block
      this->displayDataWords( block, nr_of_bytes_from_start, word_size, true );
      printf( "\n...." );

      // Next, display until end of block starting at the given offset
      nr_of_bytes = _blockSize - start_offset;
    }

  this->displayDataWords( &block[start_offset], nr_of_bytes, word_size,
                          true, start_offset );
  printf( "\n" );

  return 1;
}


void WupperParser::displayDataWords( uint8_t *bytes,
                                       uint32_t size,
                                       int      word_size,
                                       bool     display_index,
                                       uint32_t initial_index )
{
  uint8_t *byt = bytes;
  uint32_t i = initial_index;
  if( word_size == 1 )
    {
      // Display byte-by-byte
      for( ; i<size; ++i, ++byt )
        {
          if( (i & 0x1F) == 0 )
            {
              if( display_index )
                printf( "\n%4d:  ", i );
              else
                printf( "\n " );
            }
          printf( "%2x ", *byt );
        }
    }
  else
    {
      // Display as 'word_size'-byte words (little-endian)
      uint64_t word = 0;
      bool     word_completed = true;
      unsigned int bitmask = word_size-1;
      for( ; i<size; ++i, ++byt )
        {
          if( (i & 0x1F) == 0 )
            {
              if( display_index )
                printf( "\n%4d:  ", i );
              else
                printf( "\n " );
            }

          // Compile multi-byte words, then display
          word |= ((uint64_t)(*byt)) << ((i & bitmask)*8);
          word_completed = false;
          if( (i & bitmask) == bitmask )
            {
              if( word_size == 2 )
                printf( "%04lx ", word );
              else if( word_size == 4 )
                printf( "%08lx ", word );
              else
                printf( "%016lx ", word );
              // Reset word
              word = 0;
              word_completed = true;
            }
        }
      // Any remaining undisplayed data?
      if( !word_completed )
        {
          int remaining_bytes = (size-initial_index -
                                 ((size-initial_index)/word_size)*word_size);
          if( remaining_bytes == 1 )
            printf( "%02lx ", word );
          else if( remaining_bytes == 2 )
            printf( "%04lx ", word );
          else if( remaining_bytes == 3 )
            printf( "%06lx ", word );
          else
            printf( "%lx(###%d)", word, remaining_bytes );
        }
    }
}

// ----------------------------------------------------------------------------

const char *PARSER_SCA_DEV_STR[] = {
  "CONF",  "SPI",   "GPIO",
  "I2C-0", "I2C-1", "I2C-2", "I2C-3", "I2C-4", "I2C-5", "I2C-6", "I2C-7",
  "I2C-8", "I2C-9", "I2C-A", "I2C-B", "I2C-C", "I2C-D", "I2C-E", "I2C-F",
  "JTAG",  "ADC",   "DAC"
};
const char *PARSER_SCA_ERR_STR[] = {
  "????", "CHAN", "CMD", "TRID", "LEN", "DISA", "BUSY", "PROG"
};

// ----------------------------------------------------------------------------

void WupperParser::openOutputFile( std::string filename )
{
  closeOutputFile();
  _fpOut = fopen( filename.c_str(), "w" );
  if( _fpOut == 0 )
    printf( "### Failed to open output file: %s\n", filename.c_str() );
  else
    printf( "Opened output file: %s\n", filename.c_str() );
}

// ----------------------------------------------------------------------------

void WupperParser::closeOutputFile()
{
  if( _fpOut ) fclose( _fpOut );
  _fpOut = 0;
}



